#!/usr/bin/env python
# -*- coding: utf-8 -*-
import sys

sys.path.append("/opt/ros/indigo/lib/webcomm")
import rospy
import time
import json
import math
import subprocess
import re
from nav_msgs.msg import Path
from std_msgs.msg import String
from geometry_msgs.msg import Pose, PoseArray
import yaml

map_path = '/opt/ros/maps'
minDis_out_road = 1
# 最多查询网卡个数
network_Maxnum = 10


# 道路点信息解析并转发
# pose_arr = []
def road_plan_info(msg):
    # global pose_arr
    pose_arr = []
    rospy.loginfo("[clientport_road] get ROS road plan data !!")
    poses = msg.poses
    pose_arr = []
    paths = []
    plan_road =[]
    for p in poses:
        pose_new = p.pose.position
        pose_obj = {"x": round(pose_new.x, 3), "y": round(pose_new.y, 3)}
        pose_arr.append(pose_obj)

    #     road_id = pointAtLine(round(pose_new.x, 3), round(pose_new.y, 3))

    #     paths.append(road_id)
    #     if not road_id == -1:
    #         if not len(plan_road):
    #             plan_road.append(road_id)
    #             continue
    #         elif not road_id in plan_road:
    #             plan_road.append(road_id)
    #             continue
    # # print ("plan_arr",plan_road)
    # for i in plan_road:
    #     if not i == plan_road[0] or not i == plan_road[-1]:
    #         num = single_list(paths, i)
    #         if num <= 2:
    #             plan_road.remove(i)
    
    # rospy.set_param('/clientport/webcomm_param/roads', plan_road)
    # rospy.loginfo("[clientport_road] plan_road:" + str(plan_road))

    # send_data = {"marks": pose_arr, "roads": plan_road}
    # plan_road_pub.publish(json.dumps(send_data))
    return


def single_list(arr, target):
    return arr.count(target)


def all_list(arr):
    result = {}
    for i in set(arr):
        result[i] = arr.count(i)
    return result


def pointAtLine(x0, y0):
    min_dis = None
    min_id = None
    global minDis_out_road

    for road in road_lane:
        dis = None
        try:
            if road["mLaneType"] == 1:
                dis = pointToCurve(x0, y0, road["mLaneID"])
            elif road["mLaneType"] == 0:
                pos1 = get_point(road["mEndPos"])
                pos2 = get_point(road["mStartPos"])
                dis = pointToLine(pos1["x"], pos1["y"], pos2["x"], pos2["y"], x0, y0)
            if min_dis is None or dis < min_dis:
                min_dis = dis
                min_id = road["mLaneID"]
        except Exception, e:
            rospy.logerr(e)
            continue
    if min_dis >= minDis_out_road:
        return -1
    return min_id


def getPrementPath(x, y):
    path_id = pointAtLine(x, y)
    rospy.loginfo("[getPrementPath]: " + str(path_id))
    return get_lane_point(path_id)


def lineSpace(x1, y1, x0, y0):
    dis = (x1 - x0) * (x1 - x0) + (y1 - y0) * (y1 - y0)
    return math.sqrt(dis)


def pointToLine(x1, y1, x2, y2, x0, y0):
    a = lineSpace(x1, y1, x2, y2)
    b = lineSpace(x1, y1, x0, y0)
    c = lineSpace(x2, y2, x0, y0)
    if c <= 0.000001 or b <= 0.000001:
        dis = 0
        return dis
    if a <= 0.000001:
        dis = b
        return dis
    if c * c >= a * a + b * b:
        dic = b
        return dic
    if b * b >= a * a + c * c:
        dic = c
        return dic
    p = (a + b + c) * 0.5
    sqrt_data = p * (p - a) * (p - b) * (p - c)
    if sqrt_data <= 0.000001:
        return 0
    s = math.sqrt(sqrt_data)
    dic = s * 2 / a

    return dic


def pointToCurve(x0, y0, id):
    for line in road_lane:
        if line["mLaneID"] == id:
            path_data = line["mDataPath"]
            min_d = None
            for pos in path_data:
                d = lineSpace(pos["x"], pos["y"], x0, y0)
                if min_d is None or d < min_d:
                    min_d = d
            return min_d


road_mark = []
road_lane = []


# get pos with markeID
def get_point(id):
    global road_mark
    for pos in road_mark:
        if pos["markID"] == id:
            return {'x': pos["x"], "y": pos["y"]}


def get_lane_point(id):
    for line in road_lane:
        if line["mLaneID"] == id:
            return {"id": id, "start": line["mStartPos"], "end": line["mEndPos"]}


def get_map():
    global map_name
    global map_path
    global road_mark
    global road_lane
    with open(map_path + '/present_map.txt') as file:
        map_name = file.readline().split('\n')[0]
    map_path = map_path + '/' + map_name + '/' + map_name + '.mooe'

    try:
        with open(map_path) as f:
            map_read = f.read()
        map_d = json.loads(map_read)
        roads_m = map_d["mLaneMarks"]
        road_lane = map_d["mRoads"][0]["mLanes"]
        for mark in roads_m:
            if mark['mLaneMarkType'] == 0:
                mark_pos = {'markID': mark['mLaneMarkID'], 'x': round(mark['mLaneMarkXYZW']['x'], 3),
                            'y': round(mark['mLaneMarkXYZW']['y'], 3)}
                road_mark.append(mark_pos)
    except IOError, e: 
        rospy.logwarn("[clientport_road] %s",e)
    except IndexError, e:
        road_lane = []
        rospy.logwarn("[clientport_road] get_map %s",e) 
    return


pos_count = 0


def pos_callback(data):
    global pos_count
    # if webcomm_open:
    if pos_count > 10:
        poseData = {'x': 0, 'y': 0, 'a': 0, 'w': 1}
        poseData["x"] = data.position.x
        poseData["y"] = data.position.y
        poseData["a"] = data.orientation.z
        poseData["w"] = data.orientation.w
        present_path = getPrementPath(poseData["x"], poseData["y"])
        # print (present_path)
        present_road_pub.publish(json.dumps(present_path))
        rospy.loginfo("[clientport_road] presend road :" + str(present_path))

        pos_count = 0

    else:
        pos_count += 1
    return


# 获取当前信号

#   获取信号信息
def subprocessPip(cmd):
    res = subprocess.Popen(cmd, shell=True, stdout=subprocess.PIPE)
    while not res.wait():
        if not res.stderr:
            res_str = res.stdout.read()
            res_data = re.split('\.|\n', res_str)
            str_res = res_data[0]
            return str_res
        else:
            str_err = res.stderr.read()
            raise RuntimeError(str_err)


# 信号格式转化


def qualityToRssi(dbm):
    if isinstance(dbm, str):
        dbm = int(dbm)
    abs_db = abs(dbm)
    if abs_db < 0:
        return str(100)
    elif abs_db > 100:
        return str(0)
    else:
        return str(dbm + 110)


def getWlanInfo(wlan):
    if isinstance(wlan, int):
        wlan = str(wlan)
    wlan_type = "wlan" + str(wlan)
    cmd = "cat /proc/net/wireless |grep " + wlan_type + " |awk '{print $4}'"
    res_data = subprocessPip(cmd)
    if not res_data:
        return None
    if int(res_data) < 0:
        res_data = qualityToRssi(res_data)
        if not res_data:
            return None
    return res_data


# 获取
def getWlan():
    global network_Maxnum
    num = 0
    res = getWlanInfo(num)
    if not res:
        num += 1
        if num >= network_Maxnum:
            return None
        res = getWlanInfo(num)
    return {"level": res}


# 解析规划道路信息并缓存

def path_id_series(data):
    # global pose_arr
    res = yaml.load(str(data))
    poses = res["poses"]
    road_arr = []
    for p in poses:
        road_id = p["orientation"]["w"]
        data_id = int(road_id)
        road_arr.append(data_id)
    rospy.set_param('/clientport/webcomm_param/roads', road_arr)
    send_data = {"marks": [], "roads": road_arr}
    plan_road_pub.publish(json.dumps(send_data))
    rospy.loginfo("[clientport_road] road_arr:" + str(road_arr))
    return


if __name__ == '__main__':
    rospy.init_node('clientport_road', "clientport_road")
    time.sleep(5)
    webcomm_open = rospy.get_param("/webcomm_state/open_status")

    while not webcomm_open:
        if rospy.is_shutdown():
            break
        rospy.logwarn("[clientprot] webcomm is not open!!")
        time.sleep(10)
        webcomm_open = rospy.get_param("/webcomm_state/open_status")

    plan_road_pub = rospy.Publisher('/road_plan', String, queue_size=10)
    present_road_pub = rospy.Publisher('/present_road', String, queue_size=10)
    wlan_signal_pub = rospy.Publisher('/wlan_signal', String, queue_size=10)
    # 规划的道路点信息
    # rospy.Subscriber('/move_base_node/NavfnROS/plan', Path, road_plan_info)
    # 规划的道路信息
    rospy.Subscriber('/path_id_series', PoseArray, path_id_series)
    # rospy.Subscriber('/baselink_tf', Pose, pos_callback)

    get_map()
    while True:
        # rospy.loginfo("[clientport_road] is running!!!")
        time.sleep(1)
        wlan_signal = getWlan()
        wlan_signal_pub.publish(json.dumps(wlan_signal))

        if rospy.is_shutdown():
            break
